#!/usr/bin/env python


import rospy
import actionlib
from actionlib_msgs.msg import GoalStatus
from bt_tree_lib import *


class SimpleActionNode(LeafNode):
    '''
        Always waiting for next run.
    '''
    def __init__(self, name, action, action_type, goal, rate, result_timeout=1800, done_cb=None):
        super(SimpleActionNode, self).__init__(name)
        
        self.action = action
        self.action_type = action_type
        self.goal = goal
        self.tick = 1.0 / rate
        self.result_timeout = result_timeout
        
        self.user_done_cb = done_cb
        self.done_cb = self.default_done_cb
        
        self.inner_state = 0
        self.time_so_far = 0.0
        
        self.goal_statuses = ["PENDING", "ACTIVE", "PREEMPTED", "SUCCEEDED", "ABORTED", "REJECTED", "PREEMPTING", "RECALLING", "RECALLED", "LOST"]
        
        self.action_client = actionlib.SimpleActionClient(action, action_type)
        connected = self.action_client.wait_for_server(rospy.Duration(10))
        if connected:
            rospy.loginfo("%s connected to %s." % (self.name, action))
        else:
            rospy.loginfo("%s timed out connecting to %s." % (self.name, action))


    def run(self):
        if self.inner_state == 0:
            rospy.loginfo("%s sending goal..." % self.name)
            self.action_client.send_goal(self.goal, done_cb=self.done_cb)
            self.inner_state, info = 2, None
            return NodeStatus.RUNNING
        
        if self.inner_state == 2:
            self.time_so_far += self.tick
            if self.time_so_far < self.result_timeout:
                return NodeStatus.RUNNING
            else:
                rospy.loginfo("%s timed out achieving goal." % self.name)
                self.reset()
                return NodeStatus.FAILURE
        
        if self.inner_state == 4:
            self.reset()
            if self.goal_status == GoalStatus.SUCCEEDED:
                return NodeStatus.SUCCESS
            else:
                return NodeStatus.FAILURE


    def default_done_cb(self, goal_status, result):
        self.goal_status = goal_status
        rospy.loginfo("%s terminated with status %s." % (self.name, self.goal_statuses[self.goal_status]))
        
        if self.user_done_cb != None:
            self.user_done_cb(goal_status, result)
        
        self.inner_state = 4
        return


    def reset(self):
        super(SimpleActionNode, self).reset()
        self.action_client.cancel_goal()
        self.action_client.stop_tracking_goal()
        self.inner_state = 0
        self.time_so_far = 0.0
        return
        
